
#include <iostream>
#include <BPair>
#include <BMesh>
#include <BGeometry>
#include <BSoftBody>
#include "member_BShape.h"
#include "member_BBody.h"
#include "member_BSoftBody.h"
#include "ISoftBodyInternals.h"

using namespace BWE;

#define member					(*(member_BSoftBody*)_ptr)
#define member_allocate()		_ptr = new member_BSoftBody(this)
#define member_release()		delete (member_BSoftBody*)_ptr

static const BReal OVERLAP_REDUCTION_FACTOR = (BReal)0.1;
static unsigned long seed = 243703;

BSoftBody::BSoftBody()
{
	member_allocate();
	ISoftBodyCollisionShape* shape = new ISoftBodyCollisionShape(this);
	shape->setMargin(BReal(0.005));
	this->insertShape(shape);
}
BSoftBody::~BSoftBody()
{
	member_release();
}

void BSoftBody::initNodes(BShape* shape)
{
	BReal mass = 0.1;
	const BReal margin = this->shape(0)->margin();
	const BMatrix& matrix = this->matrix() * shape->matrix();
	if (BMesh* mesh = dynamic_cast<BMesh*>(shape))
	{
		const BVectorArray* vertices = mesh->vertices();
		member.vertexs.resize(vertices->size());
		for (int i = 0; i < vertices->size(); ++i)
		{
			Vertex& vertex = member.vertexs[i];
			vertex.position = vertices->at(i) * matrix;
			vertex.prepos = vertex.position;
			vertex.mass = mass;
			vertex.imass = 1 / mass;
			BSpace space(vertex.position, margin);
			vertex.leaf = member.vertex_tree.insert(space, &vertex);
		}
		updateBounds();
		const BFaceArray* faces = mesh->faces();
		for (int i = 0; i < faces->size(); i++)
		{
			const BFace& face = faces->at(i);
			member.appendLink(face.a(), face.b());
			member.appendLink(face.b(), face.c());
			member.appendLink(face.c(), face.a());
			member.appendFace(face.a(), face.b(), face.c());
		}
		return;
	}
	if (BGeometry* geometry = dynamic_cast<BGeometry*>(shape))
	{
		const BVectorArray* vertices = geometry->vertices();
		member.vertexs.resize(vertices->size());
		for (int i = 0; i < vertices->size(); ++i)
		{
			Vertex& vertex = member.vertexs[i];
			vertex.position = vertices->at(i) * matrix;
			vertex.prepos = vertex.position;
			vertex.mass = mass;
			vertex.imass = 1 / mass;
			BSpace space(vertex.position, margin);
			vertex.leaf = member.vertex_tree.insert(space, &vertex);
		}
		updateBounds();
		for (int e = 0; e < geometry->elementCount(); e++)
		{
			BElement* element = geometry->element(e);
			const BIntArray* indices = element->indices();
			if (element->primitive() == Primitive_Triangles)
			{
				for (int i = 0; i < indices->size(); i += 3)
				{
					int index0 = indices->at(i + 0);
					int index1 = indices->at(i + 1);
					int index2 = indices->at(i + 2);
					member.appendLink(index0, index1);
					member.appendLink(index1, index2);
					member.appendLink(index2, index0);
					member.appendFace(index0, index1, index2);
				}
			}
			if (element->primitive() == Primitive_Quads)
			{
				for (int i = 0; i < indices->size(); i += 4)
				{
					int index0 = indices->at(i + 0);
					int index1 = indices->at(i + 1);
					int index2 = indices->at(i + 2);
					int index3 = indices->at(i + 3);
					member.appendLink(index0, index1);
					member.appendLink(index1, index2);
					member.appendLink(index2, index3);
					member.appendLink(index3, index0);
					member.appendFace(index0, index1, index2);
					member.appendFace(index0, index2, index3);
				}
			}
		}
		return;
	}
}
void BSoftBody::randomizeConstraints()
{
	unsigned long seed = 243703;
#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
	int i, ni;

	for (i = 0, ni = member.links.size(); i < ni; ++i)
	{
		bSwap(member.links[i], member.links[NEXTRAND % ni]);
	}
	for (i = 0, ni = member.faces.size(); i < ni; ++i)
	{
		bSwap(member.faces[i], member.faces[NEXTRAND % ni]);
	}
#undef NEXTRAND
}

bool BSoftBody::checkLink(int index0, int index1) const
{
	return member.checkLink(&member.vertexs[index0], &member.vertexs[index1]);
}

void BSoftBody::appendAnchor(int index, BBody* body, const BVector& pivot, BReal influence)
{
	Anchor& anchor = member.anchors.append();
	anchor.vertex = member.vertexs.data(index);
	anchor.body = body;
	anchor.pivot = pivot;
	anchor.influence = influence;
}
void BSoftBody::appendDeformableAnchor(int index, BBody* body)
{
	VertexRigidAnchor anchor;
	Vertex& vertex = member.vertexs[index];
	const BReal ima = vertex.imass;
	const BReal imb = body_member(body)->invMass;
	BVector nrm;
	BShape* shape = body->shape(0);
	const BMatrix& matrix = body->matrix();
	BReal offset = member.worldInfo->sparsesdf.evaluate(matrix.inverse() * member.vertexs[index].position, shape, nrm, 0);

	anchor.info.body = body;
	anchor.info.normal = matrix.mult3(nrm);
	anchor.info.offset = offset;
	anchor.vertex = &member.vertexs[index];
	anchor.imass = ima;
	anchor.friction = body_member(body)->friction;
	anchor.hardness = body_member(body)->mass > REAL_EPSILON ? 1 : 0.1;
	const BVector& invInertia = body_member(body)->invInertia;
	anchor.localPoint = vertex.position - matrix.position();
	anchor.impulseMatrix = ImpulseMatrix(1, ima, imb, invInertia, anchor.localPoint);
	anchor.localPoint = body->matrix().inverse() * member.vertexs[index].position;
	member.vertexRigidAnchors.append(anchor);
}
void BSoftBody::removeAnchor(int node)
{
	const Vertex& vertex = member.vertexs[node];
	for (int i = 0; i < member.vertexRigidAnchors.size();)
	{
		const VertexRigidAnchor& contact = member.vertexRigidAnchors[i];
		if (contact.vertex == &vertex)
		{
			member.vertexRigidAnchors.remove(i);
		}
		else
		{
			i++;
		}
	}
}

void BSoftBody::addForce(const BVector& force)
{
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		addForce(force, i);
	}
}
void BSoftBody::addForce(const BVector& force, int node)
{
	Vertex& vertex = member.vertexs[node];
	if (vertex.imass > 0)
	{
		vertex.force += force;
	}
}
void BSoftBody::addAeroForceToNode(const BVector& windVelocity, int nodeIndex)
{
	const BReal dtime = member.dtime;
	const bool as_lift = member.lift_coefficient > 0;
	const bool as_drag = member.drag_coefficient > 0;
	const bool as_aero = as_lift || as_drag;
	const bool as_vaero = as_aero && (member.aero_model < F_TwoSided);

	Vertex& vertex = member.vertexs[nodeIndex];

	if (vertex.imass > 0)
	{
		Medium medium;
		member.evaluateMedium(vertex.position, medium);
		medium.velocity = windVelocity;
		medium.density = member.worldInfo->air_density;
		if (as_vaero)
		{
			const BVector rel_v = vertex.velocity - medium.velocity;
			const BReal rel_v_len = rel_v.length();
			const BReal rel_v2 = rel_v.length2();

			if (rel_v2 > REAL_EPSILON)
			{
				const BVector rel_v_nrm = rel_v.normal();
				BVector normal = vertex.normal;

				if (member.aero_model == V_TwoSidedLiftDrag)
				{
					normal *= (normal.dot(rel_v) < 0) ? (BReal)-1 : (BReal)+1;

					BReal n_dot_v = normal.dot(rel_v_nrm);
					BReal tri_area = 0.5f * vertex.area;

					BVector fDrag = (-rel_v_nrm) * (0.5f * member.drag_coefficient * medium.density * rel_v2 * tri_area * n_dot_v);

					BVector fLift;
					if (0 < n_dot_v && n_dot_v < 0.98480f)
						fLift = (normal.cross(rel_v_nrm).cross(rel_v_nrm)) * (0.5f * member.lift_coefficient * medium.density * rel_v_len * tri_area * sqrt(1 - n_dot_v * n_dot_v));

					BVector del_v_by_fDrag = fDrag * vertex.imass * member.dtime;
					BReal del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
					BReal v_len2 = vertex.velocity.length2();

					if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
					{
						BReal del_v_by_fDrag_len = del_v_by_fDrag.length();
						BReal v_len = vertex.velocity.length();
						fDrag *= BReal(0.8) * (v_len / del_v_by_fDrag_len);
					}

					vertex.force += fDrag;
					vertex.force += fLift;
				}
				else if (member.aero_model == V_Point || member.aero_model == V_OneSided || member.aero_model == V_TwoSided)
				{
					if (member.aero_model == V_TwoSided)
						normal *= (BReal)((normal.dot(rel_v) < 0) ? -1 : +1);
					const BReal dvn = rel_v.dot(normal);
					if (dvn > 0)
					{
						BVector force;
						const BReal c0 = vertex.area * dvn * rel_v2 * BReal(0.5);
						const BReal c1 = c0 * medium.density;
						force += normal * (-c1 * member.lift_coefficient);
						force += rel_v.normal() * (-c1 * member.drag_coefficient);
						ApplyClampedForce(vertex, force, dtime);
					}
				}
			}
		}
	}
}
void BSoftBody::addAeroForceToFace(const BVector& windVelocity, int faceIndex)
{
	const bool as_lift = member.lift_coefficient > 0;
	const bool as_drag = member.drag_coefficient > 0;
	const bool as_aero = as_lift || as_drag;
	const bool as_faero = as_aero && (member.aero_model >= F_TwoSided);

	if (as_faero)
	{
		Face& face = member.faces[faceIndex];

		Medium medium;

		const BVector v = (face.vertex0->velocity + face.vertex1->velocity + face.vertex2->velocity) / 3;
		const BVector center = (face.vertex0->position + face.vertex1->position + face.vertex2->position) / 3;
		member.evaluateMedium(center, medium);
		medium.velocity = windVelocity;
		medium.density = member.worldInfo->air_density;
		const BVector rel_v = v - medium.velocity;
		const BReal rel_v_len = rel_v.length();
		const BReal rel_v2 = rel_v.length2();

		if (rel_v2 > REAL_EPSILON)
		{
			const BVector rel_v_nrm = rel_v.normal();
			BVector normal = face.normal;

			if (member.aero_model == F_TwoSidedLiftDrag)
			{
				normal *= (BReal)((normal.dot(rel_v) < 0) ? -1 : +1);

				BReal n_dot_v = normal.dot(rel_v_nrm);
				BReal tri_area = 0.5f * face.area;

				BVector fLift;
				BVector fDrag = (-rel_v_nrm) * (0.5f * member.drag_coefficient * medium.density * rel_v2 * tri_area * n_dot_v);
				if (0 < n_dot_v && n_dot_v < 0.98480f)
				{
					fLift = (normal.cross(rel_v_nrm).cross(rel_v_nrm)) * (0.5f * member.lift_coefficient * medium.density * rel_v_len * tri_area * sqrt(1 - n_dot_v * n_dot_v));
				}
				fDrag /= 3;
				fLift /= 3;

				if (face.vertex0->imass > 0)
				{
					BVector del_v_by_fDrag = fDrag * face.vertex0->imass * member.dtime;
					BReal del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
					BReal v_len2 = face.vertex0->velocity.length2();
					if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
					{
						BReal del_v_by_fDrag_len = del_v_by_fDrag.length();
						BReal v_len = face.vertex0->velocity.length();
						fDrag *= BReal(0.8) * (v_len / del_v_by_fDrag_len);
					}
					face.vertex0->force += fDrag;
					face.vertex0->force += fLift;
				}
				if (face.vertex1->imass > 0)
				{
					BVector del_v_by_fDrag = fDrag * face.vertex1->imass * member.dtime;
					BReal del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
					BReal v_len2 = face.vertex1->velocity.length2();
					if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
					{
						BReal del_v_by_fDrag_len = del_v_by_fDrag.length();
						BReal v_len = face.vertex1->velocity.length();
						fDrag *= BReal(0.8) * (v_len / del_v_by_fDrag_len);
					}
					face.vertex1->force += fDrag;
					face.vertex1->force += fLift;
				}
				if (face.vertex2->imass > 0)
				{
					BVector del_v_by_fDrag = fDrag * face.vertex2->imass * member.dtime;
					BReal del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
					BReal v_len2 = face.vertex2->velocity.length2();
					if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
					{
						BReal del_v_by_fDrag_len = del_v_by_fDrag.length();
						BReal v_len = face.vertex2->velocity.length();
						fDrag *= BReal(0.8) * (v_len / del_v_by_fDrag_len);
					}
					face.vertex2->force += fDrag;
					face.vertex2->force += fLift;
				}
			}
			else if (member.aero_model == F_OneSided || member.aero_model == F_TwoSided)
			{
				if (member.aero_model == F_TwoSided)
				{
					normal *= (BReal)((normal.dot(rel_v) < 0) ? -1 : +1);
				}
				const BReal dvn = rel_v.dot(normal);
				if (dvn > 0)
				{
					BVector force;
					const BReal c0 = face.area * dvn * rel_v2;
					const BReal c1 = c0 * medium.density;
					force += normal * (-c1 * member.lift_coefficient);
					force += rel_v.normal() * (-c1 * member.drag_coefficient);
					force /= 3;
					ApplyClampedForce(*face.vertex0, force, member.dtime);
					ApplyClampedForce(*face.vertex1, force, member.dtime);
					ApplyClampedForce(*face.vertex2, force, member.dtime);
				}
			}
		}
	}
}

void BSoftBody::setVelocity(const BVector& velocity)
{
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		Vertex& vertex = member.vertexs[i];
		if (vertex.imass > 0)
		{
			vertex.velocity = velocity;
			vertex.prevel = velocity;
		}
	}
}
void BSoftBody::addVelocity(const BVector& velocity)
{
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		Vertex& vertex = member.vertexs[i];
		if (vertex.imass > 0)
		{
			vertex.velocity += velocity;
		}
	}
}
void BSoftBody::addVelocity(const BVector& velocity, int index)
{
	Vertex& vertex = member.vertexs[index];
	if (vertex.imass > 0)
	{
		vertex.velocity += velocity;
	}
}

bool BSoftBody::setVertexMass(int index, BReal mass)
{
	if (member.vertexs.check(index))
	{
		Vertex& vertex = member.vertexs[index];
		if (mass > 0)
		{
			vertex.mass = mass;
			vertex.imass = 1 / mass;
		}
		else
		{
			vertex.mass = 0;
			vertex.imass = 0;
		}
		member.dirty = true;
		return true;
	}
	return false;
}
BReal BSoftBody::vertexMass(int index) const
{
	if (member.vertexs.check(index))
	{
		const Vertex& vertex = member.vertexs[index];
		if (vertex.mass > 0)
			return vertex.mass;
	}
	return 0;
}

void BSoftBody::setMass(BReal totalMass)
{
	BBody::setMass(totalMass);
	const BReal mass = totalMass / member.vertexs.size();
	const BReal imass = 1 / mass;
	for (int i = 0; i < member.vertexs.size(); ++i)
	{
		member.vertexs[i].mass = mass;
		member.vertexs[i].imass = imass;
	}
	member.dirty = true;
}

void BSoftBody::setLinearVelocity(const BVector& linVel)
{
	BVector old_vel = getLinearVelocity();
	BVector diff = linVel - old_vel;
	for (int i = 0; i < member.vertexs.size(); ++i)
	{
		member.vertexs[i].velocity += diff;
	}
}
BVector BSoftBody::getLinearVelocity()
{
	BVector total;
	BReal mass = 0;
	for (int i = 0; i < member.vertexs.size(); ++i)
	{
		const Vertex& vertex = member.vertexs[i];
		BReal mass = vertex.imass == 0 ? 0 : 1 / vertex.imass;
		total += vertex.velocity * mass;
		if (vertex.imass > 0)
			mass += vertex.imass;
	}
	if (mass == 0)
		return total;
	return total / mass;
}

void BSoftBody::setAngularVelocity(const BVector& angVel)
{
	BVector old_vel = getLinearVelocity();
	BVector center = centerOfMass();
	for (int i = 0; i < member.vertexs.size(); ++i)
	{
		member.vertexs[i].velocity = angVel.cross(member.vertexs[i].position - center) + old_vel;
	}
}

void BSoftBody::rotate(const BQuater& rot)
{
	const BReal margin = this->shape(0)->margin();
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		Vertex& vertex = member.vertexs[i];
		vertex.position = rot * vertex.position;
		vertex.prepos = rot * vertex.prepos;
		vertex.normal = rot * vertex.normal;
		BSpace space(vertex.position, margin);
		member.vertex_tree.update(vertex.leaf, space);
	}
	updateNormals();
	updateBounds();
	member.updateConstants();
}
void BSoftBody::scale(const BVector& scale)
{
	const BReal margin = this->shape(0)->margin();
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		Vertex& vertex = member.vertexs[i];
		vertex.position *= scale;
		vertex.prepos *= scale;
		BSpace space(vertex.position, margin);
		member.vertex_tree.update(vertex.leaf, space);
	}
	updateNormals();
	updateBounds();
	member.updateConstants();
}

void BSoftBody::setPose(bool isframe)
{
	member.pose.isframe = isframe;

	const BReal omass = this->mass();
	const BReal kmass = omass * member.vertexs.size() * 1000;
	BReal tmass = omass;
	member.pose.weights.resize(member.vertexs.size());
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		Vertex& vertex = member.vertexs[i];
		if (vertex.imass <= 0)
			tmass += kmass;
	}
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		Vertex& vertex = member.vertexs[i];
		if (vertex.imass > 0)
			member.pose.weights[i] = 1 / (vertex.imass * tmass);
		else
			member.pose.weights[i] = kmass / tmass;
	}

	member.pose.center = evaluatePoseCenter();
	member.pose.positions.resize(member.vertexs.size());
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		member.pose.positions[i] = member.vertexs[i].position - member.pose.center;
	}
	member.pose.space = this->facesVolume();
	member.pose.rotation.reset();
	member.pose.scale.reset();

	BVector scalingX;
	BVector scalingY;
	BVector scalingZ;
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		const BVector& position = member.pose.positions[i];
		const BVector wp = position * member.pose.weights[i];
		scalingX += position * wp.x();
		scalingY += position * wp.y();
		scalingZ += position * wp.z();
	}
	member.pose.base_scaling.setColumn(0, scalingX);
	member.pose.base_scaling.setColumn(1, scalingY);
	member.pose.base_scaling.setColumn(2, scalingZ);
	member.pose.base_scaling = member.pose.base_scaling.inverse();
	member.updateConstants();
}

void BSoftBody::setPoseMatching(BReal matching)
{
	if (member.pose.matching != matching)
	{
		member.pose.matching = matching;
		this->emit(Signal_Changed);
	}
}
BReal BSoftBody::poseMatching() const
{
	return member.pose.matching;
}

BReal BSoftBody::facesVolume() const
{
	BReal space = 0;
	if (member.vertexs.size() > 0)
	{
		const BVector origin = member.vertexs[0].position;
		for (int i = 0; i < member.faces.size(); i++)
		{
			const Face& face = member.faces[i];
			BVector cros = (face.vertex1->position - origin).cross(face.vertex2->position - origin);
			space += (face.vertex0->position - origin).dot(cros);
		}
		space /= 6;
	}
	return space;
}
BVector BSoftBody::centerOfMass() const
{
	BVector com;
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		com += (member.vertexs[i].position * this->vertexMass(i));
	}
	com /= this->mass();
	return com;
}
BVector BSoftBody::evaluatePoseCenter() const
{
	BVector com;
	if (member.pose.isframe)
	{
		for (int i = 0; i < member.vertexs.size(); i++)
		{
			com += member.vertexs[i].position * member.pose.weights[i];
		}
	}
	return com;
}

BVector BSoftBody::clusterCom(int cluster) const
{
	return member.clusterCom(member.clusters[cluster]);
}

int BSoftBody::generateClusters(int k, int maxiterations)
{
	member.releaseClusters();
	member.clusters.resize(bMin(k, member.vertexs.size()));
	for (int i = 0; i < member.clusters.size(); ++i)
	{
		member.clusters[i] = new Cluster();
		member.clusters[i]->collide = true;
	}
	k = member.clusters.size();
	if (k > 0)
	{
		BArray<BVector> centers;
		BVector cog;
		for (int i = 0; i < member.vertexs.size(); ++i)
		{
			cog += member.vertexs[i].position;
			member.clusters[(i * 29873) % member.clusters.size()]->vertexs.append(&member.vertexs[i]);
		}
		cog /= (BReal)member.vertexs.size();
		centers.append(cog, k);
		const BReal slope = 16;
		bool changed;
		int iterations = 0;
		do
		{
			const BReal w = 2 - bMin<BReal>(1, iterations / slope);
			changed = false;
			iterations++;
			for (int i = 0; i < k; ++i)
			{
				BVector center;
				for (int j = 0; j < member.clusters[i]->vertexs.size(); ++j)
				{
					center += member.clusters[i]->vertexs[j]->position;
				}
				if (member.clusters[i]->vertexs.size())
				{
					center /= (BReal)member.clusters[i]->vertexs.size();
					center = centers[i] + (center - centers[i]) * w;
					changed |= ((center - centers[i]).length2() > REAL_EPSILON);
					centers[i] = center;
					member.clusters[i]->vertexs.resize(0);
				}
			}
			for (int i = 0; i < member.vertexs.size(); ++i)
			{
				const BVector nx = member.vertexs[i].position;
				int kbest = 0;
				BReal kdist = ClusterMetric(centers[0], nx);
				for (int j = 1; j < k; ++j)
				{
					const BReal d = ClusterMetric(centers[j], nx);
					if (d < kdist)
					{
						kbest = j;
						kdist = d;
					}
				}
				member.clusters[kbest]->vertexs.append(&member.vertexs[i]);
			}
		} while (changed && (iterations < maxiterations));

		BArray<int> cids(-1, member.vertexs.size());
		for (int i = 0; i < member.clusters.size(); ++i)
		{
			for (int j = 0; j < member.clusters[i]->vertexs.size(); ++j)
			{
				cids[int(member.clusters[i]->vertexs[j] - member.vertexs.data())] = i;
			}
		}
		for (int i = 0; i < member.faces.size(); ++i)
		{
			const int idx[] = {int(member.faces[i].vertex0 - member.vertexs.data()),
							   int(member.faces[i].vertex1 - member.vertexs.data()),
							   int(member.faces[i].vertex2 - member.vertexs.data())};
			for (int j = 0; j < 3; ++j)
			{
				const int cid = cids[idx[j]];
				for (int q = 1; q < 3; ++q)
				{
					const int kid = idx[(j + q) % 3];
					if (cids[kid] != cid)
					{
						if (member.clusters[cid]->vertexs.find(&member.vertexs[kid]) == member.clusters[cid]->vertexs.size())
						{
							member.clusters[cid]->vertexs.append(&member.vertexs[kid]);
						}
					}
				}
			}
		}

		if (member.clusters.size() > 1)
		{
			Cluster* pmaster = new Cluster();
			pmaster->collide = false;
			pmaster->vertexs.reserve(member.vertexs.size());
			for (int i = 0; i < member.vertexs.size(); ++i) pmaster->vertexs.append(&member.vertexs[i]);
			member.clusters.append(pmaster);
			bSwap(member.clusters[0], member.clusters[member.clusters.size() - 1]);
		}

		for (int i = 0; i < member.clusters.size(); ++i)
		{
			if (member.clusters[i]->vertexs.size() == 0)
			{
				member.releaseCluster(i--);
			}
		}
	}
	else
	{
		member.clusters.resize(member.faces.size());
		for (int i = 0; i < member.clusters.size(); ++i)
		{
			member.clusters[i] = new Cluster();
			member.clusters[i]->collide = true;
		}

		for (int i = 0; i < member.faces.size(); ++i)
		{
			member.clusters[i]->vertexs.append(member.faces[i].vertex0);
			member.clusters[i]->vertexs.append(member.faces[i].vertex1);
			member.clusters[i]->vertexs.append(member.faces[i].vertex2);
		}
	}

	if (member.clusters.size())
	{
		for (int i = 0; i < member.clusters.size(); ++i)
		{
			Cluster& cluster = *member.clusters[i];
			cluster.imass = 0;
			cluster.masses.resize(cluster.vertexs.size());
			for (int j = 0; j < cluster.vertexs.size(); ++j)
			{
				if (cluster.vertexs[j]->imass == 0)
				{
					cluster.containsAnchor = true;
					cluster.masses[j] = REAL_LARGE;
				}
				else
				{
					cluster.masses[j] = 1 / cluster.vertexs[j]->imass;
				}
				cluster.imass += cluster.masses[j];
			}
			cluster.imass = 1 / cluster.imass;
			cluster.com = member.clusterCom(&cluster);
			cluster.lv.set(0, 0, 0);
			cluster.av.set(0, 0, 0);
			cluster.leaf = 0;

			BMatrix& ii = cluster.locii;
			ii.setColumn(0, 0, 0, 0);
			ii.setColumn(1, 0, 0, 0);
			ii.setColumn(2, 0, 0, 0);
			{
				for (int i = 0; i < cluster.vertexs.size(); i++)
				{
					const BVector k = cluster.vertexs[i]->position - cluster.com;
					const BVector q = k * k;
					const BReal m = cluster.masses[i];
					ii[0][0] += m * (q[1] + q[2]);
					ii[1][1] += m * (q[0] + q[2]);
					ii[2][2] += m * (q[0] + q[1]);
					ii[0][1] -= m * k[0] * k[1];
					ii[0][2] -= m * k[0] * k[2];
					ii[1][2] -= m * k[1] * k[2];
				}
			}
			ii[1][0] = ii[0][1];
			ii[2][0] = ii[0][2];
			ii[2][1] = ii[1][2];

			ii = ii.inverse();

			cluster.framexform.reset();
			cluster.framexform.setPosition(cluster.com);
			cluster.framerefs.resize(cluster.vertexs.size());
			{
				for (int i = 0; i < cluster.framerefs.size(); ++i)
				{
					cluster.framerefs[i] = cluster.vertexs[i]->position - cluster.com;
				}
			}
		}

		updateClusters();

		member.clusterConnectivity.resize(member.clusters.size() * member.clusters.size());
		{
			for (int c0 = 0; c0 < member.clusters.size(); c0++)
			{
				member.clusters[c0]->clusterIndex = c0;
				for (int c1 = 0; c1 < member.clusters.size(); c1++)
				{
					bool connected = false;
					Cluster* cla = member.clusters[c0];
					Cluster* clb = member.clusters[c1];
					for (int i = 0; !connected && i < cla->vertexs.size(); i++)
					{
						for (int j = 0; j < clb->vertexs.size(); j++)
						{
							if (cla->vertexs[i] == clb->vertexs[j])
							{
								connected = true;
								break;
							}
						}
					}
					member.clusterConnectivity[c0 + c1 * member.clusters.size()] = connected;
				}
			}
		}
	}

	return (member.clusters.size());
}

void BSoftBody::predictMotion(BReal dtime)
{
	if (member.dirty)
	{
		member.dirty = false;
		member.updateConstants();
		member.updateSpaceTree();
		member.deltas.resize(member.vertexs.size());
		member.weights.resize(member.vertexs.size());
	}

	member.dtime = dtime;
	member.idtime = BReal(1) / member.dtime;
	member.velmrg = member.dtime * 3;
	if (BShape* shape = this->shape(0))
	{
		member.radmrg = shape->margin();
		member.updmrg = member.radmrg * BReal(0.25);
	}

	addVelocity(body_member(this)->gravity * member.dtime);
	applyForces();

	for (int i = 0; i < member.vertexs.size(); i++)
	{
		Vertex& vertex = member.vertexs[i];
		vertex.prepos = vertex.position;
		BVector deltaV = vertex.force * vertex.imass * member.dtime;
		BReal clampDeltaV = member.worldInfo->maxDisplacement / member.dtime;
		for (int c = 0; c < 3; c++)
		{
			if (deltaV[c] > clampDeltaV)
			{
				deltaV[c] = clampDeltaV;
			}
			if (deltaV[c] < -clampDeltaV)
			{
				deltaV[c] = -clampDeltaV;
			}
		}
		vertex.velocity += deltaV;
		vertex.position += vertex.velocity * member.dtime;
		vertex.force.reset();
	}

	updateClusters();
	updateBounds();

	for (int i = 0; i < member.vertexs.size(); i++)
	{
		Vertex& vertex = member.vertexs[i];
		BSpace volumn(vertex.position, member.radmrg);
		member.vertex_tree.update(vertex.leaf, volumn, vertex.velocity * member.velmrg, member.updmrg);
	}

	if (!member.face_tree.empty())
	{
		for (int i = 0; i < member.faces.size(); ++i)
		{
			Face& face = member.faces[i];
			const BVector velocity = (face.vertex0->velocity + face.vertex1->velocity + face.vertex2->velocity) / 3;
			BSpace volumn = SpaceOf(face, member.radmrg);
			member.face_tree.update(face.leaf, volumn, velocity * member.velmrg, member.updmrg);
		}
	}

	updatePose();

	if (member.pose.isframe && (member.pose.matching > 0))
	{
		for (int i = 0; i < member.vertexs.size(); i++)
		{
			Vertex& vertex = member.vertexs[i];
			if (vertex.imass > 0)
			{
				const BVector position = member.pose.rotation * member.pose.positions[i] + member.pose.center;
				vertex.position = bLerp(vertex.position, position, member.pose.matching);
			}
		}
	}
	member.vertexContacts.reset();
	member.multi_contacts.reset();
	member.vertexRigidAnchors.reset();
	member.faceVertexContacts.reset();
	member.faceRigidContacts.reset();
	member.soft_contacts.reset();
}

void BSoftBody::solveAnchors()
{
	for (int i = 0; i < member.anchors.size(); i++)
	{
		Anchor& anchor = member.anchors[i];
		BBody* body = anchor.body;
		if (Vertex* vertex = anchor.vertex)
		{
			const BMatrix& matrix = body->matrix();

			const BVector localPoint = matrix * anchor.pivot;
			BReal invMass = body_member(body)->invMass;
			const BVector& invInertia = body_member(body)->invInertia;

			BMatrix impulseMatrix = ImpulseMatrix(member.dtime, vertex->imass, invMass, invInertia, localPoint);

			const BVector point = matrix * anchor.pivot;
			const BVector va = body->velocityInPoint(localPoint) * member.dtime;
			const BVector vb = vertex->position - vertex->prepos;
			const BVector vr = (va - vb) + (point - vertex->position) * member.anchor_hardness;
			const BVector impulse = impulseMatrix * vr * anchor.influence;
			vertex->position += impulse * vertex->imass * member.dtime;
			body->applyImpulse(-impulse, localPoint);
		}
	}
}
void BSoftBody::solveContacts()
{
	const BReal margin = this->shape(0)->margin();
	for (int i = 0; i < member.vertexContacts.size(); i++)
	{
		const VertexContact& contact = member.vertexContacts[i];
		if (BBody* body = contact.info.body)
		{
			BVector velocity = body->velocityInPoint(contact.localPoint) * member.dtime;
			const BVector offset = contact.vertex->position - contact.vertex->prepos;
			const BVector vr = offset - velocity;
			const BReal dn = vr.dot(contact.info.normal);
			if (dn <= REAL_EPSILON)
			{
				const BReal dp = bMin((contact.vertex->position.dot(contact.info.normal) + contact.info.offset), margin);
				const BVector fv = vr - (contact.info.normal * dn);
				const BVector impulse = contact.impulseMatrix * vr - (fv * contact.friction) + (contact.info.normal * (dp * contact.hardness));
				contact.vertex->position -= impulse * contact.vertex->imass * member.dtime;
				body->applyImpulse(impulse, contact.localPoint);
			}
		}
	}
	for (int i = 0; i < member.faceRigidContacts.size(); i++)
	{
		FaceRigidContact& contact = member.faceRigidContacts[i];
		if (BBody* body = contact.info.body)
		{
			const BMatrix& matrix = body->matrix();
			for (int v = 0; v < 3; v++)
			{
				Vertex* vertex = contact.face->vertex(v);
				BVector localPoint = contact.point - matrix.position();
				BVector velocity = body->velocityInPoint(localPoint) * member.dtime;
				const BVector offset = vertex->position - vertex->prepos;
				const BVector vr = offset - velocity;
				const BReal dn = vr.dot(contact.info.normal);
				if (dn <= REAL_EPSILON)
				{						
					const BReal dp = bMin((vertex->position.dot(contact.info.normal) + contact.info.offset), margin);
					const BVector fv = vr - (contact.info.normal * dn);
					const BVector impulse = contact.impulseMatrix * vr - (fv * contact.friction) + (contact.info.normal * (dp * contact.hardness));
					vertex->position -= impulse * vertex->imass * member.dtime;
					body->applyImpulse(impulse, localPoint);
				}
			}
		}
	}

	if (member.multi_contacts.size())
	{

	}
	if (member.soft_contacts.size())
	{
		for (int i = 0; i < member.soft_contacts.size(); ++i)
		{
			const SoftContact& contact = member.soft_contacts[i];
			Vertex& vertex = *contact.node;
			Face& face = *contact.face;
			const BVector p = BaryEval(face.vertex0->position, face.vertex1->position, face.vertex2->position, contact.weights);
			const BVector q = BaryEval(face.vertex0->prepos, face.vertex1->prepos, face.vertex2->prepos, contact.weights);
			const BVector vr = (vertex.position - vertex.prepos) - (p - q);
			BVector corr;
			BReal dot = vr.dot(contact.normal);
			if (dot < 0)
			{
				const BReal j = contact.margin - (contact.normal.dot(vertex.position) - contact.normal.dot(p));
				corr += contact.normal * j;
			}
			corr -= ProjectOnPlane(vr, contact.normal) * contact.friction;
			vertex.position += corr * contact.forceMixing[0];
			face.vertex0->position -= corr * (contact.forceMixing[1] * contact.weights.x());
			face.vertex1->position -= corr * (contact.forceMixing[1] * contact.weights.y());
			face.vertex2->position -= corr * (contact.forceMixing[1] * contact.weights.z());
		}
	}
}
void BSoftBody::solveLinks()
{
	for (int i = 0; i < member.links.size(); i++)
	{
		Link& link = member.links[i];
		BReal imaimb = link.vertex0->imass + link.vertex1->imass;
		if (imaimb > 0)
		{
			const BVector vec = link.vertex1->position - link.vertex0->position;
			const BReal len2 = vec.length2();
			if (link.length2 + len2 > REAL_EPSILON)
			{
				BReal k = (link.length2 - len2) / ((link.length2 + len2) * imaimb);
				link.vertex0->position -= vec * (k * link.vertex0->imass);
				link.vertex1->position += vec * (k * link.vertex1->imass);
			}
		}
	}
}

void BSoftBody::pointersToIndices()
{
#define PTR2IDX(_p_, _b_) reinterpret_cast<Vertex*>((_p_) - (_b_))
	Vertex* base = member.vertexs.size() ? member.vertexs.data() : 0;
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		if (member.vertexs[i].leaf)
		{
			member.vertexs[i].leaf->value = i;
		}
	}
	for (int i = 0; i < member.links.size(); i++)
	{
		member.links[i].vertex0 = PTR2IDX(member.links[i].vertex0, base);
		member.links[i].vertex1 = PTR2IDX(member.links[i].vertex1, base);
	}
	for (int i = 0; i < member.faces.size(); i++)
	{
		member.faces[i].vertex0 = PTR2IDX(member.faces[i].vertex0, base);
		member.faces[i].vertex1 = PTR2IDX(member.faces[i].vertex1, base);
		member.faces[i].vertex2 = PTR2IDX(member.faces[i].vertex2, base);
		if (member.faces[i].leaf)
		{
			member.faces[i].leaf->value = i;
		}
	}
	for (int i = 0; i < member.anchors.size(); i++)
	{
		member.anchors[i].vertex = PTR2IDX(member.anchors[i].vertex, base);
	}
#undef PTR2IDX
}
void BSoftBody::indicesToPointers(const int* map)
{
#define IDX2PTR(_p_, _b_) map ? (&(_b_)[map[(((char*)_p_) - (char*)0)]]) : (&(_b_)[(((char*)_p_) - (char*)0)])
	Vertex* base = member.vertexs.size() ? member.vertexs.data() : 0;
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		if (member.vertexs[i].leaf)
		{
			member.vertexs[i].leaf->value = &member.vertexs[i];
		}
	}
	for (int i = 0; i < member.links.size(); i++)
	{
		member.links[i].vertex0 = IDX2PTR(member.links[i].vertex0, base);
		member.links[i].vertex1 = IDX2PTR(member.links[i].vertex1, base);
	}
	for (int i = 0; i < member.faces.size(); i++)
	{
		member.faces[i].vertex0 = IDX2PTR(member.faces[i].vertex0, base);
		member.faces[i].vertex1 = IDX2PTR(member.faces[i].vertex1, base);
		member.faces[i].vertex2 = IDX2PTR(member.faces[i].vertex2, base);
		if (member.faces[i].leaf)
		{
			member.faces[i].leaf->value = &member.faces[i];
		}
	}
	for (int i = 0; i < member.anchors.size(); i++)
	{
		member.anchors[i].vertex = IDX2PTR(member.anchors[i].vertex, base);
	}
#undef IDX2PTR
}

DbvntNode* CopyToDbvnt(const SpaceNode* node)
{
	if (node == 0)
		return 0;
	DbvntNode* root = new DbvntNode(node);
	if (node->next)
	{
		DbvntNode* c0 = CopyToDbvnt(node->prev);
		root->prev = c0;
		DbvntNode* c1 = CopyToDbvnt(node->next);
		root->next = c1;
	}
	return root;
}

void CalculateNormalCone(DbvntNode* root)
{
	if (!root)
		return;
	if (root->isLeaf())
	{
		const Face* face = root->value;
		root->normal = face->normal;
		root->angle = 0;
	}
	else
	{
		BVector n0;
		BVector n1;
		BReal a0 = 0;
		BReal a1 = 0;
		if (root->prev)
		{
			CalculateNormalCone(root->prev);
			n0 = root->prev->normal;
			a0 = root->prev->angle;
		}
		if (root->next)
		{
			CalculateNormalCone(root->next);
			n1 = root->next->normal;
			a1 = root->next->angle;
		}
		root->normal = (n0 + n1).normal();
		root->angle = bMax(a0, a1) + n0.angle(n1) * (BReal)0.5;
	}
}

void BSoftBody::updateNormals()
{
	BVector zv;
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		member.vertexs[i].normal = zv;
	}
	for (int i = 0; i < member.faces.size(); i++)
	{
		Face& face = member.faces[i];
		face.center = (face.vertex0->position + face.vertex1->position + face.vertex2->position) * 0.333333333333333;
		face.normal = (face.vertex1->position - face.vertex0->position).cross(face.vertex2->position - face.vertex0->position);
		face.normal.normalize();
		face.vertex0->normal += face.normal;
		face.vertex1->normal += face.normal;
		face.vertex2->normal += face.normal;
	}
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		BReal len = member.vertexs[i].normal.length();
		if (len > REAL_EPSILON)
		{
			member.vertexs[i].normal /= len;
		}
	}
}
void BSoftBody::updatePoints()
{
	BShape* shape = this->shape(0);
	if (BMesh* mesh = dynamic_cast<BMesh*>(shape))
	{
		BVectorArray* vertices = mesh->vertices();
		BVectorArray* normals = mesh->normals();
		for (int i = 0; i < member.vertexs.size(); i++)
		{
			BVector vertex = member.vertexs[i].position;
			BVector normal = member.vertexs[i].normal;
			vertices->set(i, BVector(vertex.x(), vertex.y(), vertex.z()));
			normals->set(i, BVector(normal.x(), normal.y(), normal.z()));
		}
		mesh->smooth();
		mesh->dirty();
		mesh->fresh();
		return;
	}
	if (BGeometry* geometry = dynamic_cast<BGeometry*>(shape))
	{
		BMatrix matrix = this->matrix();
		BVectorArray* vertices = geometry->vertices();
		BVectorArray* normals = geometry->normals();
		for (int i = 0; i < member.vertexs.size(); i++)
		{
			BVector vertex = member.vertexs[i].position;
			BVector normal = member.vertexs[i].normal;
			vertices->set(i, BVector(vertex.x(), vertex.y(), vertex.z()) * matrix.inverse());
			normals->set(i, BVector(normal.x(), normal.y(), normal.z()));
		}
		geometry->smooth();
		geometry->dirty();
		geometry->fresh();
		return;
	}
}
void BSoftBody::updateBounds()
{
	if (member.vertexs.size())
	{
		member.space.reset(member.vertexs[0].position);
		for (int i = 1; i < member.vertexs.size(); i++)
		{
			member.space.expand(member.vertexs[i].position);
		}
		const BReal margin = this->shape(0)->margin();
		member.space.min() -= margin;
		member.space.max() += margin;
		member.worldInfo->broadphase->updateProxy(this);
	}
	else
	{
		member.space.reset();
	}
}
void BSoftBody::updatePose()
{
	if (member.pose.isframe)
	{
		member.pose.center = evaluatePoseCenter();
		BVector vec0;
		BVector vec1;
		BVector vec2;
		for (int i = 0; i < member.vertexs.size(); i++)
		{
			const BVector a = (member.vertexs[i].position - member.pose.center) * member.pose.weights[i];
			const BVector& b = member.pose.positions[i];
			vec0 += b * a.x();
			vec1 += b * a.y();
			vec2 += b * a.z();
		}
		BMatrix Apq;
		Apq.setColumn(0, vec0);
		Apq.setColumn(1, vec1);
		Apq.setColumn(2, vec2);
		BMatrix r;
		BMatrix s;
		PolarDecompose(Apq, r, s);
		member.pose.rotation = r;
		member.pose.scale = member.pose.base_scaling * r.reverse() * Apq;
		if (member.maxvolume > 1)
		{
			BVector v0 = member.pose.scale.column(0);
			BVector v1 = member.pose.scale.column(1);
			BVector v2 = member.pose.scale.column(2);
			BReal determinant = v0.triple(v1, v2);
			const BReal idet = Clamp(1 / determinant, 1, member.maxvolume);
			member.pose.scale = Mul(member.pose.scale, idet);
		}
	}
}

void BSoftBody::updateClusters()
{
	for (int i = 0; i < member.clusters.size(); ++i)
	{
		Cluster& cluster = *member.clusters[i];
		if (cluster.vertexs.size())
		{
			const BReal eps = BReal(0.0001);
			BVector vec0(eps * 1, 0, 0);
			BVector vec1(0, eps * 2, 0);
			BVector vec2(0, 0, eps * 3);
			cluster.com = member.clusterCom(&cluster);
			for (int j = 0; j < cluster.vertexs.size(); ++j)
			{
				const BVector a = cluster.vertexs[j]->position - cluster.com;
				const BVector& b = cluster.framerefs[j];
				vec0 += b * a[0];
				vec1 += b * a[1];
				vec2 += b * a[2];
			}
			BMatrix m;
			m.setColumn(0, vec0);
			m.setColumn(1, vec1);
			m.setColumn(2, vec2);
			BMatrix r, s;
			PolarDecompose(m, r, s);
			cluster.framexform.setPosition(cluster.com);
			cluster.framexform.setColumn(0, r.column(0));
			cluster.framexform.setColumn(1, r.column(1));
			cluster.framexform.setColumn(2, r.column(2));

			cluster.lv.set(0, 0, 0);
			cluster.av.set(0, 0, 0);
			for (int j = 0; j < cluster.vertexs.size(); ++j)
			{
				const BVector v = cluster.vertexs[j]->velocity * cluster.masses[j];
				cluster.lv += v;
				cluster.av += (cluster.vertexs[j]->position - cluster.com).cross(v);
			}
			cluster.lv = cluster.lv * cluster.imass * (1 - cluster.linear_damping);
			cluster.av = cluster.invInertia * cluster.av * (1 - cluster.angular_damping);
			cluster.vimpulses[0] = cluster.vimpulses[1] = BVector();
			cluster.dimpulses[0] = cluster.dimpulses[1] = BVector();
			cluster.nvimpulses = 0;
			cluster.ndimpulses = 0;

			if (cluster.matching > 0)
			{
				for (int j = 0; j < cluster.vertexs.size(); ++j)
				{
					Vertex* vertex = cluster.vertexs[j];
					const BVector x = cluster.framexform * cluster.framerefs[j];
					vertex->position = bLerp(vertex->position, x, cluster.matching);
				}
			}

			if (cluster.collide)
			{
				BVector min = cluster.vertexs[0]->position;
				BVector max = min;
				for (int j = 1; j < cluster.vertexs.size(); ++j)
				{
					SetMin(min, cluster.vertexs[j]->position);
					SetMax(max, cluster.vertexs[j]->position);
				}
				BSpace bounds(min, max);
				if (cluster.leaf)
					member.cluster_dbvt.update(cluster.leaf, bounds, cluster.lv * member.dtime * 3, member.radmrg);
				else
					cluster.leaf = member.cluster_dbvt.insert(bounds, &cluster);
			}
		}
	}
}
void BSoftBody::cleanupClusters()
{
	for (int i = 0; i < member.joints.size(); ++i)
	{
		member.joints[i]->terminate();
	}
	member.joints.clear();
}
void BSoftBody::prepareClusters(int iterations)
{
	for (int i = 0; i < member.joints.size(); ++i)
	{
		member.joints[i]->Prepare(member.dtime, iterations);
	}
}

void BSoftBody::applyForces()
{
	const bool as_lift = member.lift_coefficient > 0;
	const bool as_drag = member.drag_coefficient > 0;
	const bool as_pressure = member.pressure_coefficient != 0;
	const bool as_volume = member.volume_coefficient > 0;
	const bool as_aero = as_lift || as_drag;

	const bool use_medium = as_aero;
	const bool use_volume = as_pressure || as_volume;
	BReal ivolumetp = 0;
	BReal dvolumetv = 0;
	Medium medium;
	if (use_volume)
	{
		BReal volume = this->facesVolume();
		ivolumetp = BReal(1) / fabs(volume) * member.pressure_coefficient;
		dvolumetv = (member.pose.space - volume) * member.volume_coefficient;
	}

	for (int i = 0; i < member.vertexs.size(); ++i)
	{
		Vertex& vertex = member.vertexs[i];
		if (vertex.imass > 0)
		{
			if (use_medium)
			{
				addAeroForceToNode(member.windVelocity, i);
			}
			if (as_pressure)
			{
				vertex.force += vertex.normal * (vertex.area * ivolumetp);
			}
			if (as_volume)
			{
				vertex.force += vertex.normal * (vertex.area * dvolumetv);
			}
		}
	}

	for (int i = 0; i < member.faces.size(); i++)
	{
		addAeroForceToFace(member.windVelocity, i);
	}
}
void BSoftBody::solveClusters(BReal sor)
{
	for (int i = 0; i < member.joints.size(); i++)
	{
		member.joints[i]->Solve(member.dtime, sor);
	}
}
void BSoftBody::applyClusters(bool drift)
{
	if (member.clusters.size())
	{
		member.deltas.fill(BVector());
		member.weights.fill(0);
		if (drift)
		{
			for (int i = 0; i < member.clusters.size(); ++i)
			{
				Cluster* cluster = member.clusters[i];
				if (cluster->ndimpulses)
				{
					cluster->dimpulses[0] /= (BReal)cluster->ndimpulses;
					cluster->dimpulses[1] /= (BReal)cluster->ndimpulses;
				}
			}
		}
		for (int i = 0; i < member.clusters.size(); ++i)
		{
			Cluster* cluster = member.clusters[i];
			if (0 < (drift ? cluster->ndimpulses : cluster->nvimpulses))
			{
				const BVector v = (drift ? cluster->dimpulses[0] : cluster->vimpulses[0]) * member.dtime;
				const BVector w = (drift ? cluster->dimpulses[1] : cluster->vimpulses[1]) * member.dtime;
				for (int j = 0; j < cluster->vertexs.size(); ++j)
				{
					const int idx = int(cluster->vertexs[j] - member.vertexs.data());
					const BVector& position = cluster->vertexs[j]->position;
					const BReal mass = cluster->masses[j];
					member.deltas[idx] += (v + w.cross(position - cluster->com)) * mass;
					member.weights[idx] += mass;
				}
			}
		}
		for (int i = 0; i < member.vertexs.size(); ++i)
		{
			if (member.weights[i] > 0)
			{
				member.vertexs[i].position += member.deltas[i] / member.weights[i];
			}
		}
	}
}
void BSoftBody::dampClusters()
{
	for (int i = 0; i < member.clusters.size(); ++i)
	{
		Cluster* cluster = member.clusters[i];
		if (cluster->node_damping > 0)
		{
			for (int j = 0; j < cluster->vertexs.size(); ++j)
			{
				Vertex* node = cluster->vertexs[j];
				if (node->imass > 0)
				{
					const BVector vx = cluster->lv + cluster->av.cross(cluster->vertexs[j]->prepos - cluster->com);
					if (vx.length2() <= node->velocity.length2())
					{
						node->velocity += (vx - node->velocity) * cluster->node_damping;
					}
				}
			}
		}
	}
}
void BSoftBody::applyRepulsionForce(BReal timeStep, bool applySpringForce)
{
	BArray<int> indices(member.faceVertexContacts.size());
	for (int i = 0; i < member.faceVertexContacts.size(); ++i)
		indices[i] = i;
#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
	for (int i = 0; i < indices.size(); i++)
	{
		bSwap(indices[i], indices[NEXTRAND % indices.size()]);
	}
	for (int k = 0; k < member.faceVertexContacts.size(); ++k)
	{
		int idx = indices[k];
		FaceVertexContact& contact = member.faceVertexContacts[idx];
		Vertex* vertex = contact.vertex;
		Face* face = contact.face;
		BVector l = vertex->position - BaryEval(face->vertex0->position, face->vertex1->position, face->vertex2->position, contact.bary);
		BReal d = contact.margin - contact.normal.dot(l);
		d = bMax(BReal(0), d);

		const BVector& va = vertex->velocity;
		BVector vb = BaryEval(face->vertex0->velocity, face->vertex1->velocity, face->vertex2->velocity, contact.bary);
		BVector vr = va - vb;
		const BReal vn = vr.dot(contact.normal);
		if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep)
			continue;
		BVector vt = vr - contact.normal * vn;
		BReal I = 0;
		BReal mass = vertex->imass == 0 ? 0 : 1 / vertex->imass;
		if (applySpringForce)
			I = -bMin(member.repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
		if (vn < 0)
			I += BReal(0.5) * mass * vn;
		int face_penetration = 0;
		face_penetration |= face->vertex0->penetration;
		face_penetration |= face->vertex1->penetration;
		face_penetration |= face->vertex2->penetration;

		int node_penetration = vertex->penetration;
		BReal tilde = (BReal)(2.0 * I / (1.0 + contact.bary.length2()));

		if (face_penetration > 0 || node_penetration > 0)
		{
			tilde *= 2.0;
		}
		if (face_penetration <= 0)
		{
			face->vertex0->velocity += contact.normal * contact.bary[0] * tilde * vertex->imass;
			face->vertex1->velocity += contact.normal * contact.bary[1] * tilde * vertex->imass;
			face->vertex2->velocity += contact.normal * contact.bary[2] * tilde * vertex->imass;
		}
		if (node_penetration <= 0)
		{
			vertex->velocity -= contact.normal * tilde * vertex->imass;
		}

		BReal vt_norm = vt.length();
		if (vt_norm > REAL_EPSILON)
		{
			BReal delta_vn = -2 * I * vertex->imass;
			BReal vt_new = bMax(1 - contact.friction * delta_vn / (vt_norm + REAL_EPSILON), BReal(0)) * vt_norm;
			I = BReal(0.5) * mass * (vt_norm - vt_new);
			vt.normalize();
			tilde = BReal(2 * I / (1 + contact.bary.length2()));

			if (face_penetration > 0 || node_penetration > 0)
				tilde *= 2;
			if (face_penetration <= 0)
			{
				face->vertex0->velocity += vt * contact.bary[0] * tilde * (face->vertex0)->imass;
				face->vertex1->velocity += vt * contact.bary[1] * tilde * (face->vertex1)->imass;
				face->vertex2->velocity += vt * contact.bary[2] * tilde * (face->vertex2)->imass;
			}
			if (node_penetration <= 0)
			{
				vertex->velocity -= vt * tilde * vertex->imass;
			}
		}
	}
}
void BSoftBody::applyVelocity()
{
	for (int i = 0; i < member.vertexs.size(); i++)
	{
		Vertex& vertex = member.vertexs[i];
		vertex.velocity = (vertex.position - vertex.prepos) * member.idtime * 0.96;
		vertex.force.reset();
	}
}

void BSoftBody::setSelfCollision(bool selfCollision)
{
	if (member.selfCollision != selfCollision)
	{
		member.selfCollision = selfCollision;
		this->emit(Signal_Changed);
	}
}
bool BSoftBody::selfCollision() const
{
	return member.selfCollision;
}

void BSoftBody::collisionHandler(BBody* body, BShape* shape, const BMatrix& matrix)
{
	const BReal margin = this->shape(0)->margin();
	BSpace space = shape_member(shape)->transSpace(matrix);
	space.min() -= margin;
	space.max() += margin;
	if (member.vertex_tree.collide(member.vertex_tree.root(), space, member.result))
	{
		for (int i = 0; i < member.result.size(); i++)
		{
			Vertex* node = member.result[i]->value;
			member.collisionBody(node, body, shape, matrix, margin);
		}
	}
	processClusters(this, body, shape, matrix);
	if (member.face_tree.collide(member.face_tree.root(), space, member.result))
	{
		for (int i = 0; i < member.result.size(); i++)
		{
			Face* face = member.result[i]->value;
			member.collisionBody(face, body, shape, matrix, margin);
		}
	}
}
void BSoftBody::processClusters(BSoftBody* softBody, BBody* body, BShape* shape, const BMatrix& matrix)
{
	BReal margin = shape->margin() + softBody->shape(0)->margin();
	BReal friction = bMin(softBody->friction(), body->friction());
	BSpace space = shape_member(shape)->transSpace(matrix);
	space.min() -= margin;
	space.max() += margin;
	ISpaceTree cluster_dbvt = softbody_member(softBody)->cluster_dbvt;
	cluster_dbvt.collide(cluster_dbvt.root(), space, member.result);
	for (int i = 0; i < member.result.size(); i++)
	{
		const SpaceNode* leaf = member.result[i];
		Cluster* cluster = leaf->value;

		if (body_member(body)->mass <= REAL_EPSILON && cluster->containsAnchor)
			return;

		SoftClusterCollisionShape cshape(cluster);
		BShape* convexShape = dynamic_cast<BShape*>(shape);

		IGjkEpaSolver::Result result;
		if (IGjkEpaSolver::SignedDistance(&cshape, BMatrix(), convexShape, matrix, BVector(1, 0, 0), result))
		{
			CJoint joint;
			if (member.SolveContact(result, cluster, body, joint, margin, friction))
			{
				CJoint* pj = new CJoint();
				*pj = joint;
				softbody_member(softBody)->joints.append(pj);
				if (body_member(body)->mass > REAL_EPSILON)
				{
					pj->factor *= softbody_member(softBody)->kSRHR_CL;
					pj->split *= softbody_member(softBody)->kSR_SPLT_CL;
				}
				else
				{
					pj->factor *= softbody_member(softBody)->kSKHR_CL;
					pj->split *= softbody_member(softBody)->kSK_SPLT_CL;
				}
			}
		}
	}
}

void BSoftBody::collisionSoftBody(BSoftBody* softBody)
{
	if (this == softBody)
		return;
	{
		BReal margin = this->shape(0)->margin() + softBody->shape(0)->margin();
		BReal friction = bMin(this->friction(), softBody->friction());
		member.cluster_dbvt.collide(member.cluster_dbvt.root(), softbody_member(softBody)->cluster_dbvt.root(), member.spacePairs);
		for (int i = 0; i < member.spacePairs.size(); i++)
		{
			SpacePair& pair = member.spacePairs[i];
			member.process(pair.nodeA, pair.nodeB, this, softBody, margin, friction);
		}
	}
	{
		BReal margin = shape(0)->margin() + softBody->shape(0)->margin();
		{
			member.vertex_tree.collide(member.vertex_tree.root(), softbody_member(softBody)->face_tree.root(), member.spacePairs);
			for (int i = 0; i < member.spacePairs.size(); i++)
			{
				SpacePair& pair = member.spacePairs[i];
				member.collisionSoftBody(pair, this, softBody, margin);
			}
		}
		{
			ISpaceTree vertex_tree = softbody_member(softBody)->vertex_tree;
			vertex_tree.collide(vertex_tree.root(), member.face_tree.root(), member.spacePairs);
			for (int i = 0; i < member.spacePairs.size(); i++)
			{
				SpacePair& pair = member.spacePairs[i];
				member.collisionSoftBody(pair, this, softBody, margin);
			}
		}
	}
	if (softBody->state() != BBody::State_Sleeping || this->state() != BBody::State_Sleeping)
	{
		if (this != softBody)
		{
			BReal margin = shape(0)->margin() + softBody->shape(0)->margin();
			{
				bool useFaceNormal = (softbody_member(softBody)->faces.size() > 0);
				member.vertex_tree.collide(member.vertex_tree.root(), softbody_member(softBody)->face_tree.root(), member.spacePairs);
				for (int i = 0; i < member.spacePairs.size(); i++)
				{
					SpacePair& pair = member.spacePairs[i];
					member.collisionSoftBody(pair, this, softBody, margin, useFaceNormal);
				}
			}
			{
				bool useFaceNormal = (member.faces.size() > 0);
				softbody_member(softBody)->vertex_tree.collide(softbody_member(softBody)->vertex_tree.root(), member.face_tree.root(), member.spacePairs);
				for (int i = 0; i < member.spacePairs.size(); i++)
				{
					SpacePair& pair = member.spacePairs[i];
					member.collisionSoftBody(pair, softBody, this, margin, useFaceNormal);
				}
			}
		}
		else
		{
			if (softBody->selfCollision())
			{
				BReal margin = 2 * shape(0)->margin();
				bool useFaceNormal = (member.faces.size() > 0);
				CalculateNormalCone(member.face_dbvnt);
				member.face_tree.selfCollide(member.face_dbvnt, member.dbvntPairs);
				for (int i = 0; i < member.dbvntPairs.size(); i++)
				{
					DbvntPair& pair = member.dbvntPairs[i];
					member.collisionSoftBody(pair, this, softBody, margin, useFaceNormal);
				}
			}
		}
	}
}

void BSoftBody::setWindVelocity(const BVector& velocity)
{
	if (member.windVelocity != velocity)
	{
		member.windVelocity = velocity;
		this->emit(Signal_Changed);
	}
}
const BVector& BSoftBody::windVelocity() const
{
	return member.windVelocity;
}

void BSoftBody::getAabb(BVector& aabbMin, BVector& aabbMax) const
{
	aabbMin = member.space.min();
	aabbMax = member.space.max();
}

void BSoftBody::updateDeactivating(BReal timeStep)
{
	if (this->state() == State_Sleeping)
		return;

	if (member.maxSpeedSquared < member.sleepingThreshold * member.sleepingThreshold)
	{
		body_member(this)->deactivatingTime += timeStep;
	}
	else
	{
		body_member(this)->deactivatingTime = 0;
		this->setState(BBody::State_Active);
	}
}

